#include "graphbuilder.h"
#include "../mapobjects/sampleobjects.h"
#include "../mapobjects/mapobject.h"
#include <stdio.h>

void GraphBuilder::init(Viewer * viewer)
{
    this->viewer = viewer;
    connect ( viewer, SIGNAL ( pointClicked ( const QPointF & )), 
		this, SLOT ( onClickEvent ( const QPointF & )));
	connect ( viewer, SIGNAL ( mapLoaded ()), this, SLOT ( onMapLoaded()));
    startIndex = -1;

}

void GraphBuilder::onClickEvent ( const QPointF & pt )
{
    if(startIndex < 0)
    {
        startIndex = nearestPt(pt);
        path.clear();
        path.push_back(pt);
    }
    else
    {
        int finishIndex = nearestPt(pt);
        shortestPath(path, startIndex, finishIndex);
        path.push_back(pt);
        
        MapObject* mapObject = new Route ();
        Polyline * polyline = new Polyline ( path, mapObject );
        mapObject -> addPrimitive ( polyline );
        viewer -> addCosmetics ( mapObject );
        viewer -> update ();
        startIndex = -1;
    }
}

void GraphBuilder::onMapLoaded ()
{
    list<QLineF> lines;
	ObjectStorage * storage = viewer -> getStorage();

	vector<MapObject *> mapObjects;
	storage -> getMapObjectListOfType ( std::string ( "Road" ), mapObjects );
	for ( size_t i = 0; i < mapObjects.size(); ++i )
	{
		vector<PrimitiveBase *> polylines;
		mapObjects[i] -> getPrimitiveListOfType ( e_PrimTypePolyline, polylines );
		for ( size_t j = 0; j < polylines.size(); ++j )
		{
			Polyline * polyline = reinterpret_cast<Polyline *> ( polylines[j] );
			vector<QPointF> points;
			polyline -> getPoints ( points );
            for(size_t k = 1; k < points.size(); ++k)
            {               
                addLine(QLineF(points[k - 1], points[k]), lines);
                
            }
		}
	}



    for(list<QLineF>::iterator it = lines.begin(); it != lines.end(); ++it)
    {
        size_t first = -1, second = -1;

        for(size_t i = 0; i != points.size(); ++i)
        {
            if(pointDistance(it->p1(), points[i]) <= POINT_EPS)
                first = i;

            if(pointDistance(it->p2(), points[i]) <= POINT_EPS)
                second = i;
        }

        if(-1 == first)
        {
            first = points.size();
            graph.push_back(vector<size_t>());
            points.push_back(it->p1());
        }

        if(-1 == second)
        {
            second = points.size();
            graph.push_back(vector<size_t>());
            points.push_back(it->p2());
        }

        graph[first].push_back(second);
        graph[second].push_back(first);
    }

    FILE* file = fopen("c:\\graph.txt", "w");
    for(int i = 0; i != graph.size(); ++i)
    {
        for(int j = 0; j != graph[i].size(); ++j)
            fprintf(file,"%i ", graph[i][j]);
        fprintf(file,"\n");
    }
    fclose(file);
}

void GraphBuilder::addLine(const QLineF& line, list<QLineF>& lines)
{
    if(lines.empty())
        lines.push_back(line);
    else
    {
        vector<QPointF> crossPoints(1, line.p2());

        for(list<QLineF>::iterator it = lines.begin(); it != lines.end(); ++it)
        {
            QPointF cross;
                      
            if(QLineF::BoundedIntersection == line.intersect(QLineF(it->p1(), it->p2()), &cross))
            {
                if(pointDistance(it->p1(), cross) > POINT_EPS)
                    lines.push_front(QLineF(it->p1(), cross));

                if(pointDistance(it->p2(), cross) > POINT_EPS)
                    lines.push_front(QLineF(it->p2(), cross));

                it = lines.erase(it);

                if(pointDistance(line.p1(), cross) > POINT_EPS &&
                    pointDistance(line.p2(), cross) > POINT_EPS)
                    crossPoints.push_back(cross);
            }

            if(it == lines.end())
                break;
        }

        sortPoints(line.p1(), crossPoints);

        lines.push_front(QLineF(line.p1(), crossPoints[0]));
        for(int i = 1; i != crossPoints.size(); ++i)
        {
            if(!(crossPoints[i - 1] == crossPoints[i]))
                lines.push_front(QLineF(crossPoints[i - 1],crossPoints[i]));
        }
    }
}

void GraphBuilder::sortPoints(const QPointF& first, vector<QPointF>& pts)
{
    for(int i = 0; i != pts.size() - 1; ++i)
    {
        for(int j = i+1; j != pts.size(); ++j)
        {
            if(pow(pts[j].x() - first.x(), 2.0) + pow(pts[j].y() - first.y(), 2.0) <
                pow(pts[i].x() - first.x(), 2.0) + pow(pts[i].y() - first.y(), 2.0))
            {
                QPointF buf = pts[j];
                pts[j] = pts[i];
                pts[i] = buf;
            }
        }
    }
}

double GraphBuilder::pointDistance(const QPointF& lhs, const QPointF& rhs)
{
    return sqrt(pow(lhs.x() - rhs.x(), 2.0) + pow(lhs.y() - rhs.y(), 2.0));
}

void GraphBuilder::shortestPath(vector<QPointF>& path, const size_t s, const size_t f)
{

    std::vector<bool> used(graph.size(), false);
    std::vector<int> previous(points.size(), s);
    std::vector<double> distance(points.size(), 1000);
    for(size_t i = 0; i != graph[s].size(); ++i)
        distance[graph[s][i]] = pointDistance(points[graph[s][i]], points[s]);
    distance[s] = 0;

    used[s] = true;
    previous[s] = -1;

    for(;;)
    {
        int min = 0;
        for(min = 0; min != graph.size(); ++min)
        {
            if(!used[min])
                break;
        }

        if(min == graph.size())
            break;

        for(int i = min; i != graph.size(); ++i)
        {
            if(!used[i] && distance[min] > distance[i])
                min = i;
        }

        used[min] = true;

        for(int j = 0; j != graph[min].size(); ++j)
        {
            double d = pointDistance(points[min],points[graph[min][j]]);
            if(distance[min] + d < distance[graph[min][j]])
            {
                distance[graph[min][j]] = distance[min] + d;
                previous[graph[min][j]] = min;
            }
        }

    }

    int j = f;
    vector<QPointF> buf;
    buf.push_back(points[f]);
    while(previous[j] != -1)
    {
        buf.push_back(points[j]);
        j = previous[j];
    }
    buf.push_back(points[s]);

    for(int i = buf.size() - 1; i != -1; --i)
        path.push_back(buf[i]);
    
}

int GraphBuilder::nearestPt(const QPointF& pt)
{
    int minIndex = 0;
    double minDistance = pointDistance(pt, points[0]);
    for(int i = 1; i != points.size(); ++i)
    {
        double dist = pointDistance(pt, points[i]);
        if(dist < minDistance)
        {
            minDistance = dist;
            minIndex = i;
        }
    }
    return minIndex;
}